%%计算辨识结果
%% mass:质量  r: 质心位置 
%% Ic = [I_5  I_8   I_9
%%       I_8  I_6   I_10
%%       I_9  I_10  I_7 ];

% robot = importrobot("/home/maoli/Desktop/Dynamic_ParaEstimation/leg_v3_right_serial_cali.urdf");



mass = zeros(1,5);
r =zeros(3,5);
Ic = zeros(3,3,5);
Ij = zeros(3,3,5);
S_r =zeros(3,3,5);

%%辨识参数
%%右腿
% X=[0.571899993809635;-0.000514709999972864;-1.36064586445035e-17;0.00177288999998434;0.000649329956001059;-1.19999992966468e-07;5.57760077942056e-06;0.000395865202488028;5.79999995112291e-08;0.000370274209018634;0.576735386567467;0.00169937303238695;-0.0105883131472379;5.57445056449876e-05;0.000559225522406957;2.65868658660881e-05;2.51142996286619e-06;0.000649372536847532;7.33530950091232e-06;0.000611512166676237;1.40152178332530;0.00370763744179040;-0.00103770334847181;-0.0180345478600306;0.00399358704618321;0.000349694755033515;-0.000230478328157500;0.00452646306071069;-0.000126318842248663;0.00237310357943218;0.297777859126245;0.00527412851638584;0.000671015820677712;-0.0276971187051527;0.00681665202582885;0.000183072508345129;0.000375695652999401;0.00814235052990353;0.000288972212404015;0.00140922047874566;0.0939174763646100;0.000932324797747696;1.93099695915802e-05;-0.00326237496582550;0.000192631899776825;3.29305499380508e-06;6.91578959966720e-05;0.000280961331273576;1.74196997045025e-06;8.88542335701253e-05;0.0753959581806109;4.21968571718221e-10;0.116803153347541;5.97568527015091e-10;0.152621271760831;3.63084123002288e-10;0.170079624291104;0.00879276684084811;0.0274353833710108;0.0388818910156898];
%%左腿
X=[0.571899999955765;-0.000514710000000042;2.08599029363428e-20;0.00177289000000000;0.000649329959006756;-1.20000000012860e-07;5.57760100038333e-06;0.000395865197993060;5.80000000001449e-08;0.000358007507385275;0.572451523955597;0.00176450466763195;0.0104749271827109;4.13624125327377e-06;0.000549923046298819;-2.85659778813579e-05;-2.02650984515886e-07;0.000649392594295532;3.37870697805015e-07;0.000591531591966167;1.41783576168934;0.00266803688994840;0.00117083746748715;-0.0178903559636938;0.00373353974294829;9.54944936240176e-06;-0.000169200373633351;0.00358265936214566;2.48229415285052e-05;0.00188863111168924;0.264530723494062;0.00598519137206745;-0.000551038607324744;-0.0284286706191777;0.00528950967179793;2.84940812006585e-05;0.000568417916353077;0.00569618755716064;4.91594072710952e-06;0.000414074861071619;0.0792246798942622;0.000986739440547888;-5.26995286805734e-06;-0.00305851596790306;0.000186103087609142;1.67951955922798e-07;6.98791600527014e-05;0.000265829629301911;2.07043316245456e-07;8.38923410986801e-05;0.102881978339845;0.0305335462144259;0.189727033588077;0.0262528853318628;0.0747458874720857;0.0316375373864488;0.178123596321309;0.0620683280412181;0.0562555380365313;0.0692394794535696];
%%urdf参数
% X=[0.571900000000000;-0.000514710000000000;0;0.00177289000000000;0.000649329959000000;-1.20000000000000e-07;5.57760100000000e-06;0.000395865198000000;5.80000000000000e-08;0.000356428239000000;0.571900000000000;0.00177289000000000;-0.0105229600000000;0;0.000549587464000000;2.86391760000000e-05;5.80000000000000e-08;0.000649329959000000;1.20000000000000e-07;0.000589024423000000;1.34580000000000;0.00215328000000000;-0.00134580000000000;-0.0174954000000000;0.00370501600000000;3.58228000000000e-06;-0.000188221360000000;0.00346547344800000;-3.58964000000000e-05;0.00184487804800000;0.245000000000000;0.00475275500000000;-4.01800000000000e-05;-0.0298802000000000;0.00504095578152000;5.29451820000000e-07;0.000588972999800000;0.00517846488624500;-4.97935280000000e-06;0.000194923283765000;0.131856000000000;0.00338368867200000;0;-0.00554797305600000;0.000286842514304256;0;0.000112106084563072;0.000487413733005120;4.47000000000000e-10;0.000224543218700864];
for i =1:5
    mass(i) = X(10*(i-1)+1);
    r(1,i) = X(10*(i-1)+2)/mass(i);
    r(2,i) = X(10*(i-1)+3)/mass(i);
    r(3,i) = X(10*(i-1)+4)/mass(i);
    
    Ij(1,1,i) =X(10*(i-1)+5);
    Ij(2,2,i) =X(10*(i-1)+8);
    Ij(3,3,i) =X(10*(i-1)+10);
    Ij(1,2,i) =X(10*(i-1)+6);
    Ij(2,1,i) =X(10*(i-1)+6);
    Ij(1,3,i) =X(10*(i-1)+7);
    Ij(3,1,i) =X(10*(i-1)+7);
    Ij(2,3,i) =X(10*(i-1)+9);
    Ij(3,2,i) =X(10*(i-1)+9);
    
    S_r(:,:,i) = skew_matrix(r(:,i));
 
end


I_urdf = zeros(5,6);

for i=1:5
    Ic(:,:,i) = Ij(:,:,i) - mass(i)*S_r(:,:,i)*S_r(:,:,i)'
    I_urdf(i,1) = Ic(1,1,i);
    I_urdf(i,2) = Ic(1,2,i);
    I_urdf(i,3) = Ic(1,3,i);
    I_urdf(i,4) = Ic(2,2,i);
    I_urdf(i,5) = Ic(2,3,i);
    I_urdf(i,6) = Ic(3,3,i);    
end

X_urdf = zeros(60,1);
for i = 1:5
    X_urdf(10*(i-1)+1:10*(i-1)+10,1) = [mass(i) r(1,i) r(2,i) r(3,i) I_urdf(i,1) I_urdf(i,2) I_urdf(i,3) I_urdf(i,4) I_urdf(i,5) I_urdf(i,6)]; 
end
X_urdf(51:60,1) = X(51:60,1);
XX_urdf = reshape(X_urdf,10,6);
for i =1:5
    XX_urdf(11,i) = XX_urdf((i-1)*2 +1,6);
    XX_urdf(12,i) = XX_urdf((i-1)*2 +2,6);
end
XXX_urdf =XX_urdf(:,1:5);
writematrix(XXX_urdf, 'left_leg.csv');
